#include "gps.h"
#include "Utility.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "mainlogic.h"

extern uint8_t gpsUpdated;
extern uint8_t init;
extern uint8_t stepOK;
extern uint8_t gpsStatus;

uint8_t doNmeaCheckSum(char *line, int size) {
	int sum = 0;
	uint8_t result = 0;
	int i;
	if(line[size-5] == '*') {
		sum = htoi(line+size-4,2);
		//printf("%c%c %02x\r\n",line[size-4],line[size-3],sum);
		result = line[1];
		for(i=2;i<size-5;i++) {
			result = result ^ line[i];
		}
		//printf("%02x  %02x\r\n",sum,result);
	}
	return result==sum;
}

StrRange findStr(char *line, int size,int num) {
	StrRange reg;
	int i = 0;
	int findNum = 0;
	reg.start = -1;
	reg.end = -1;
	reg.isOk = 0;
	for(i = 0; i<size; i++) {
		if(line[i]==',') {
			findNum++;
			if(findNum == num) {
				reg.start = i+1;
			}
			else if(findNum>num) {
				reg.end = i;
				reg.isOk = reg.end>reg.start;
				break;
			}
		}
	}
	return reg;
}

#pragma pack(1)
typedef struct 
{
	uint32_t towMS;
	uint32_t towSubMS;
	int32_t qError;
	uint16_t week;
	uint8_t flag;
	uint8_t refInfo;
} UBX_TIM_TP;


typedef struct
{
	uint8_t subCode;
	uint8_t mode;
	uint8_t nc;
	uint8_t selfSurveyProgress;
	uint32_t nc1;
	uint16_t nc2;
	uint16_t monitorAlarm;
	uint8_t gpsstatus;
	uint8_t nc3;
	uint8_t spare1;
	uint8_t spare2;
	float cbase;
	float cbaseRate;
	uint32_t nc4;
	float nc5;
	float temp;
	double lat;
	double lon;
	double alt;
	float qError;
	uint8_t nc6;
} TSIP_8FAC;

typedef struct
{
	uint8_t subCode;
	uint32_t gpsWeek;
	uint16_t gpsWeekNum;
	int16_t utcOffset;
	uint8_t timeFlag;
	uint8_t second;
	uint8_t min;
	uint8_t hour;
	uint8_t day;
	uint8_t month;
	uint16_t year;
} TSIP_8FAB;

#pragma pack()

/*
$GPGSA,<1>,<2>,<3>,<3>,,,,,<3>,<3>,<3>,<4>,<5>,<6>,<7><CR><LF>
<1>模式 ：M = 手动， A = 自动。
<2>定位型式 1 = 未定位， 2 = 二维定位， 3 = 三维定位。
<3>PRN 数字：01 至 32 表天空使用中的卫星编号，最多可接收12颗卫星信息。
<4> PDOP位置精度因子（0.5~99.9）
<5> HDOP水平精度因子（0.5~99.9）
<6> VDOP垂直精度因子（0.5~99.9）
<7> Checksum.(检查位).
$GPRMC,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>*hh<CR><LF>
<1> UTC时间，hhmmss（时分秒）格式
<2> 定位状态，A=有效定位，V=无效定位
<3> 纬度ddmm.mmmm（度分）格式（前面的0也将被传输）
<4> 纬度半球N（北半球）或S（南半球）
<5> 经度dddmm.mmmm（度分）格式（前面的0也将被传输）
<6> 经度半球E（东经）或W（西经）
<7> 地面速率（000.0~999.9节，前面的0也将被传输）
<8> 地面航向（000.0~359.9度，以真北为参考基准，前面的0也将被传输）
<9> UTC日期，ddmmyy（日月年）格式
<10> 磁偏角（000.0~180.0度，前面的0也将被传输）
<11> 磁偏角方向，E（东）或W（西）
<12> 模式指示（仅NMEA0183 3.00版本输出，A=自主定位，D=差分，E=估算，N=数据无效）
*/
int qErrorOff = 0;
int preError = 0;
int qErrorInit = 0;

void doUbxPro(uint8_t* line, int size)
{
	//printf("TTTTTTTTTTTTTTTTTTTTTTTT\n");
		if(size > 16 && line[0]==0xB5 && line[1]==0x62 && line[2]==0x0D && line[3]==0x01)
		{
			UBX_TIM_TP *tp = (UBX_TIM_TP *)(line+6);
			//printf("tp qerr = %d\n", tp->qError);
			if(qErrorInit)
			{
				qErrorOff = tp->qError - preError;
			}
			qErrorInit = 1;
			preError = tp->qError;
		}
		else
		{
			printf("other %x %x\n", line[0], line[1]);
		}
}

void doTSip(uint8_t* line, int size)
{
	//printf("++++++++++++++++++++++++++++++++++++++++++++++msg id=%x %x size=%d\n", line[1], line[2],size);
	if(line[1] == 0x8F && line[2] == 0xAC && size==72)
	{
		TSIP_8FAC *p = (TSIP_8FAC *)(line+2);
		/*uint8_t *tmp = (uint8_t *)&(p->temp);
		uint8_t sw;
		sw = tmp[0];
		tmp[0] = tmp[3];
		tmp[3] = sw;
		sw = tmp[1];
		tmp[1] = tmp[2];
		tmp[2]=sw; */
		//float tmp = p->temp;
		//p->temp = BigLittleSwap32(tmp);
		BigLittleSwitch((uint8_t *)&p->temp,4);
		BigLittleSwitch(&p->lat,8);
		BigLittleSwitch(&p->lon,8);
		BigLittleSwitch(&p->qError,4);
		p->lat = p->lat * 180/ M_PI;
		p->lon = p->lon * 180/ M_PI;
		if(p->gpsstatus == 0)
		{
			g_gpsStatus.valid = 1;
		}
		if(qErrorInit)
			{
				qErrorOff = 1000*(p->qError - preError);
			}
		qErrorInit = 1;
		preError = p->qError;
			printf("tp qerr = %f\n", p->qError);
		//printf("sub=%2x tmp = %.2f lat=%lf lon=%lf qError=%f gpsstus=%d\n",p->subCode, p->temp, p->lat, p->lon, p->qError, p->gpsstatus);
	}
	else if(line[1] == 0x8f && line[2] == 0xAB)
	{
		TSIP_8FAB *p = (TSIP_8FAB *)(line+2);
		BigLittleSwitch(&p->year,2);
		//printf("%d-%d-%d %d:%d:%d\n", p->year, p->month, p->day, p->hour, p->min, p->second);
	}
	/*if(size == 73)
	{
	printf("stip\n");
	for(int i=0;i<size;i++)
	{
		printf("%2x ", line[i]);
	}
	printf("\nend\n");
	}*/
	
}

void Gps_Conv_Date(char *line)
{
	char buf[3];
	buf[2] = 0;
	buf[0] = line[0];
	buf[1] = line[1];
	g_gpsStatus.day = atoi(buf);
	buf[0] = line[2];
	buf[1] = line[3];
	g_gpsStatus.month = atoi(buf);
	buf[0] = line[4];
	buf[1] = line[5];
	g_gpsStatus.year = atoi(buf)+2000;
}

void Gps_Conv_Time(char *line)
{
	char buf[3];
	buf[2] = 0;
	buf[0] = line[0];
	buf[1] = line[1];
	g_gpsStatus.hour = atoi(buf);
	buf[0] = line[2];
	buf[1] = line[3];
	g_gpsStatus.min = atoi(buf);
	buf[0] = line[4];
	buf[1] = line[5];
	g_gpsStatus.sec = atoi(buf);
	g_gpsStatus.timeValid = 1;
}

void doGpsCmd(uint8_t* line, int size) {
	gpsUpdated = 1;
	if(!g_gpsStatus.valid)
	{
		printf("%s\n",line);
	}
	if(size > 6) {
		if(size > 16 && line[0]==0xB5 && line[1]==0x62 && line[2]==0x0D && line[3]==0x01)
		{
			//todo，此处不应该出现，需要解决
			doUbxPro(line,size);
		}
		if(strncmp((char *)line+3,"GSA",3) == 0) {
			if(doNmeaCheckSum((char *)line,size)) {
				char *opt[25];
				int spcnt = 0;
	      ParseCmd((char *)line, opt, 25, &spcnt, ",");
				if(spcnt > 15)
				{
					int t = atoi(opt[2]);
					if(t == 3)
					{
							g_gpsStatus.type = GpsFixType3D;
							g_gpsStatus.valid = 1;
					}
					else if(t == 2)
					{
							g_gpsStatus.type = GpsFixType2D;
							g_gpsStatus.valid = 1;
					}
					else
					{
							g_gpsStatus.type = GpsFixTypeNone;
							g_gpsStatus.valid = 0;
					}
				}
			}
			if(g_gpsStatus.type == GpsFixType2D) {
				//printf("FixedType=2D\r\n");
			}
			else if(g_gpsStatus.type == GpsFixType3D) {
				//printf("FixedType=3D\r\n");
			}
			else {
				printf("FixedType=None\r\n");
				init = 20;
			}
		}
		else if(strncmp((char *)line+3, "GGA",3) == 0)
		{
			if(doNmeaCheckSum((char *)line,size)) {
				char *opt[25];
				int spcnt = 0;
	      ParseCmd((char *)line, opt, 25, &spcnt, ",");
				if(spcnt > 14)
				{
					Gps_Conv_Time(opt[1]);
					g_gpsStatus.lat = strtod(opt[2],NULL);
					g_gpsStatus.lon = strtod(opt[4],NULL);
					g_gpsStatus.latDir = opt[3][0];
					g_gpsStatus.lonDir = opt[5][0];
					g_gpsStatus.satCount = atoi(opt[7]);
					int tmp = atoi(opt[6]);
					{
						if(tmp != 0)
						{
							g_gpsStatus.valid = 1;
						}
						else
						{
							g_gpsStatus.valid = 0;
						}
					}
					//printf("GGA valid=%d lat=%lf lon=%lf time = %2d:%2d:%2d sat=%d\n",g_gpsStatus.valid, g_gpsStatus.lat, g_gpsStatus.lon, g_gpsStatus.hour, g_gpsStatus.min, g_gpsStatus.sec, g_gpsStatus.satCount); 
				}
			}
		}
		else if(strncmp((char *)line+3,"RMC",3) == 0) {
			if(doNmeaCheckSum((char *)line,size)) {
				char *opt[25];
				int spcnt = 0;
	      ParseCmd((char *)line, opt, 25, &spcnt, ",");
				if(spcnt > 10)
				{
					Gps_Conv_Time(opt[1]);
					Gps_Conv_Date(opt[9]);
					g_gpsStatus.lat = strtod(opt[3],NULL);
					g_gpsStatus.lon = strtod(opt[5],NULL);
					g_gpsStatus.latDir = opt[4][0];
					g_gpsStatus.lonDir = opt[6][0];
					if(opt[2][0] == 'A')
					{
						g_gpsStatus.valid = 1;
					}
					else
					{
						g_gpsStatus.valid = 0;
					}
					//printf("time = %2d:%2d:%2d date=%4d-%2d-%2d lat=%lf %c lon=%lf %c valid=%d\n", g_gpsStatus.hour, g_gpsStatus.min, g_gpsStatus.sec, g_gpsStatus.year, g_gpsStatus.month, g_gpsStatus.day, \
						g_gpsStatus.lat, g_gpsStatus.latDir, g_gpsStatus.lon, g_gpsStatus.lonDir, g_gpsStatus.valid);
				}
			}
		}
	}
}

